#pragma once

class pid
{
private:
    typedef pid self;
public:
    enum PID_MODE
    {
        DELTA_PID,
        POSITION_PID
    };
public:
    pid(PID_MODE mode, float p, float i, float d, float limit_h, float limit_l)
        : _mode(mode)
        , _p(p)
        , _i(i)
        , _d(d)
        , _limit_h(limit_h)
        , _limit_l(limit_l)
    {}

    int calculate()
    {
        _error[0] = _target - _now;

        // 增量式
        if (_mode == DELTA_PID)
        {
            _p_out = _error[0] - _error[1];
            _i_out = _error[0];
            _d_out = _error[0] - 2 * _error[1] + _error[2];
            _out += _p * _p_out + _i * _i_out + _d * _d_out;
        }
        // 位置式
        if (_mode == POSITION_PID)
        {
            _p_out = _p * _error[0];
            _i_out += _i * _error[0];
            _d_out = _d * (_error[0] - _error[1]);
            _out = _p_out + _i_out + _d_out;
        }
        
        _error[2] = _error[1];
        _error[1] = _error[0];
        
        // 输出限幅
        if (_out >= _limit_h)
            _out = _limit_h;
        if (_out <= _limit_l)
            _out = _limit_l;
        return _out;
    }
public:
    PID_MODE _mode;
    float _p;
    float _i;
    float _d;
    int _target;
    int _now;
    int _error[3];

    int _p_out;
    int _i_out;
    int _d_out;
    int _out;

    int _limit_h;
    int _limit_l;
};
